#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>
#include <tcr100/ImuData.h>
#include <tcr100/serial.h>
#include <sensor_msgs/Imu.h>
#include "tf/transform_broadcaster.h"

serial::Serial ser; //声明串口对象
#define BUFFERSIZE 50
unsigned char buff[BUFFERSIZE],counter;
unsigned char sign=0;
float acc_x,acc_y,acc_z,vth_x,vth_y,vth_z,roll,pitch,yaw;

unsigned char check_yaw(unsigned char *buffer)
{
  unsigned char ret,sum1,sum2,sum3;
for(int k=0;k<BUFFERSIZE;k++)
{
  if(buffer[k]==0x55&&buffer[k+1]==0x51)
  {
      sum1=buffer[k]+buffer[k+1]+buffer[k+2]+buffer[k+3]+buffer[k+4]+buffer[k+5]+buffer[k+6]+buffer[k+7]+buffer[k+8]+buffer[k+9];
      sum2=buffer[k+11]+buffer[k+12]+buffer[k+13]+buffer[k+14]+buffer[k+15]+buffer[k+16]+buffer[k+17]+buffer[k+18]+buffer[k+19]+buffer[k+20];
      sum3=buffer[k+22]+buffer[k+23]+buffer[k+24]+buffer[k+25]+buffer[k+26]+buffer[k+27]+buffer[k+28]+buffer[k+29]+buffer[k+30]+buffer[k+31];
      if((sum1==buffer[k+10])&&(sum2==buffer[k+21])&&(sum3==buffer[k+32]))//
      {
          ret=1;
          acc_x=(short(buffer[k+3]<<8| buffer[k+2]))/32768.0*16*9.8;//m/s2
          acc_y=(short(buffer[k+5]<<8| buffer[k+4]))/32768.0*16*9.8;//m/s2
          acc_z=(short(buffer[k+7]<<8| buffer[k+6]))/32768.0*16*9.8;//m/s2

          vth_x=(short(buffer[k+14]<<8| buffer[k+13]))/32768.0*2000*0.017453292222;//rad/s
          vth_y=(short(buffer[k+16]<<8| buffer[k+15]))/32768.0*2000*0.017453292222;//rad/s
          vth_z=(short(buffer[k+18]<<8| buffer[k+17]))/32768.0*2000*0.017453292222;//rad/s

          roll=(short(buffer[k+25]<<8| buffer[k+24]))/32768.0*M_PI;//rad
          pitch=(short(buffer[k+27]<<8| buffer[k+26]))/32768.0*M_PI;//rad
          yaw=(short(buffer[k+29]<<8| buffer[k+28]))/32768.0*M_PI;//rad
//          printf("acc_x=:%f\n",acc_x);
//          printf("v_th=:%f\n", vth_z);
//          printf("angle=:%f\n",yaw);
      }
      else
      {
          ret=0;
      }
  }
}
  return ret;
}
int main (int argc, char** argv){
    ros::init(argc, argv, "serial_example_node");
    ros::NodeHandle nh;
    ros::Publisher yaw_pub = nh.advertise<tcr100::ImuData>("yaw_data", 1000);
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1000);

    try
    {
        ser.setPort("/dev/ttyS1");
        ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if(ser.isOpen()){
        ROS_INFO_STREAM("Open port success. Serial Port initialized");
    }else{
        return -1;
    }
    ros::Rate loop_rate(200);
    while(ros::ok()){
        tcr100::serial result;
        ros::spinOnce();

        if(ser.available()){
            ser.read(buff,BUFFERSIZE);
            if(check_yaw(buff)!=0)
            {
            }
            sensor_msgs::Imu imu_data;
            tcr100::ImuData yaw_data;
            tf::Quaternion q;
            q.setRPY(roll,pitch,yaw);
            imu_data.header.stamp=ros::Time::now();
            imu_data.header.frame_id="imu_link";
            imu_data.angular_velocity.x=vth_x;
            imu_data.angular_velocity.y=vth_y;
            imu_data.angular_velocity.z=vth_z;
           // imu_data.angular_velocity_covariance={0.02, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.02};

            imu_data.linear_acceleration.x=acc_x;
            imu_data.linear_acceleration.y=acc_y;
            imu_data.linear_acceleration.z=acc_z;
           // imu_data.linear_acceleration_covariance={0.04, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.04};


            imu_data.orientation.x=q[0];
            imu_data.orientation.y=q[1];
            imu_data.orientation.z=q[2];
            imu_data.orientation.w=q[3];
           //imu_data.orientation_covariance={0.0025, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0025};


            yaw_data.yaw=yaw*180/M_PI;

            imu_pub.publish(imu_data);
            yaw_pub.publish(yaw_data);
            memset(buff,0,BUFFERSIZE);
        }
        loop_rate.sleep();
    }
}
